% Florian Meyer, 22/04/15.

function [] = printAll(agentPos, anchorPos, agentPosteriors, rmse, animated)
[agentNum, ~, numTimesteps] = size(agentPos);

mycolors = [ ...
    0.00,0.30,0.70; ...
    0.60,0.90,0.16; ...
    0.99,0.34,0.00; ...
    0.54,0.80,0.99; ...    
    0.92,0.75,0.33; ...
    0.66,0.00,0.00; ...    
    0.00,0.00,0.00; ...   
    ];

for timestep = 1:numTimesteps-1
    
    if(~animated)
        tmp = numTimesteps-1;
    else
        tmp = timestep;
    end
    
    figure(1);
    subplot(2,1,1)    
    scatter(anchorPos(:,1),anchorPos(:,2),30,mycolors(7,:))
    hold on
    axis ([-101 101 -101 101]);
    xlabel('x-coordinate [m]'), ylabel('y-coordinate [m]');
    pbaspect('manual')
    set(gca,'YTick',-100:50:100)
    set(gca,'XTick',-100:50:100)
    
    
    
    for agent=1:agentNum
        scatter(agentPos(agent,1,1),agentPos(agent,2,1),30,mycolors(agent,:));
        p1 = plot(squeeze(agentPos(agent,1,1:tmp)), squeeze(agentPos(agent,2,1:tmp)),'-k','LineWidth',1);
        set(p1(1),'color',mycolors(agent,:));
    end
    
    subplot(2,1,2)
    p1 = plot(1:tmp, rmse(1,1:tmp));
    set(p1(1),'color',mycolors(1,:));
    axis ([0 numTimesteps 0 50]);
    xlabel('time step'), ylabel('RMSE');
    hold on
    for agent = 2:agentNum
        p1 = plot(1:tmp, rmse(agent,1:tmp));
        set(p1(1),'color',mycolors(agent,:));
    end
    
    if(timestep == 1)
        pause
    end
    
    subplot(2,1,1)
    scatter(agentPosteriors(:,1,1,tmp),agentPosteriors(:,2,1,tmp),6,mycolors(1,:));
    for agent = 2:agentNum
        scatter(agentPosteriors(:,1,agent,tmp),agentPosteriors(:,2,agent,tmp),6,mycolors(agent,:));
    end
    hold off
    
    
    if(~animated)
        continue
    else
        pause(0.01)
    end
    
end

end